function AoT=createTree(origin,W,H,L,minAsp,maxAsp,rotationMat,swt,stp,name)
%% used to create and And-Or Tree , created in Jan 27, 2011 by wenze
%% to test, call
%[orLayer, andLayer, termLayer]=createTree(2,2,3,[0.2,0.2,0.2],[5,5,5],zeros(3,3));
%{
Members of a tree
orLayer
andLayer
termLayer
planeList % denoting the planes shared by vols inside the rootVol;
%}
orLayer = cell(W*H*L+1,1);
keyOrLayer = cell(size(orLayer));

termLayer= cell(W*H*L+1,1);

andLayer = cell(W*H*L+1,1);

planeList =[]; % struct array, denoting the shared planes in the root vol
ntList = []; % cell, same dimension as planeList, save the key (nt) of each plane;
%% create Or Layer
disp('generating or layer');
% loop over vol size

for w = 1:W
    for h = 1:H
        for l = 0:L
            % check the aspect ratio
            %{
            if l ==0
                if( (w/h < minAsp(1)) || (w/h > maxAsp(1)))
                    continue;
                end
            else
                if( (w/h < minAsp(1)) || (w/h > maxAsp(1)) ...
                        || (h/l < minAsp(2)) || (h/l > maxAsp(2))...
                        || (l/w < minAsp(3)) || (l/w) > maxAsp(3) )
                    continue;
                end
            end
            %}
            % check the volume
           if strcmp(name,'_head') || strcmp(name,'_tail')	    
                if (h<2)||(w<3)
		  continue;
	       end
	   elseif strcmp(name,'_lid')
		if (h<2)||(w<6)
		   continue;
		end
           else
               if (h<3)||(w<4)
                  continue;
               end
           end
         
		   %{
	   if (h<th_H) ||(w<th_W)
		continue;
	    end
			   %}
            % inject node
            layer = [];
            parfor x = 0:W-w
                for y = 0:H-h
                    for z = 0:L-l
                        % creat and insert into the or Layer
                        vol =createVol([x y z]',[w,h,l]','or');
                        %orLayer{h*w*l+1}=[orLayer{h*w*l+1} vol];
                        layer=[layer vol];
                    end
                end
            end
            orLayer{h*w*l+1}=[orLayer{h*w*l+1} layer];
            
        end
    end
end

keyOrLayer = cellfun(@createKey,orLayer,'UniformOutput',false);


%% create Terminal Layer, and associate with plane
disp('generating terminal layer...')
% a. replicate or layer, and change type to term;
termLayer=cellfun(@changeVolArrayToTerm,orLayer,'UniformOutput',false);
% c. associate with planes, can not be paralled because elements in the
% planeList are assumed to be unique;
for iLevel = 1:length(termLayer);
    for iVol = 1:length(termLayer{iLevel})
        vol = termLayer{iLevel}(iVol) ;
        [vol,planeList,ntList] = addPlane(vol,planeList,ntList,stp);
        termLayer{iLevel}(iVol)=vol;
    end
end


%% create AND node Layer;
disp('generating and layer...')
for iLevel = length(orLayer):-1:2; % skip layer of volume 0 and 1
    and_layer = [];
    for iVol = 1:length(orLayer{iLevel})
        orNode = orLayer{iLevel}(iVol);
        children = orNode.children;
        childVols =cell(0,0);
        x = orNode.corner(1);
        y = orNode.corner(2);
        z = orNode.corner(3);
        
        w = orNode.dim(1);
        h = orNode.dim(2);
        l = orNode.dim(3);
        
        for xp = x+1: x+w-1
            childVols{end+1}=[x;y;z;xp-x;h;l;xp;y;z;w-(xp-x);h;l];
        end
        for yp = y+1:y+h-1
            childVols{end+1}=[x,y,z,w,yp-y,l,x,yp,z,w,h-(yp-y),l]';
        end
        
        for zp = z+1:z+l-1;
            childVols{end+1}=[x,y,z,w,h,zp-z,x,y,zp,w,h,l-(zp-z)]';
        end
        
        % alow dim (3)==0, and the 0 depth plane should be in front
        zp = z+l;
        childVols{end+1}=[x,y,z,w,h,l-1,x,y,zp,w,h,0]';
        
        % convert to orNode index
        for jVol = 1:length(childVols)
            index=[];
            for i = 1:2
                node = childVols{jVol}((i-1)*6+1:i*6);
                if sum(abs(node))==0
                    index = [0;0;];
                    continue;
                end
                jLevel = prod(node(4:6))+1;
                keyNode = keyOrLayer{jLevel};
                ind = findMember(keyNode,node');
                if length(ind)==1
                    index =[index;jLevel;ind];
                elseif isempty(ind)
                    index = [index; 0;0]; % the vol's asp does not satisfyt the requirement
                else
                    disp('error, more than one or node has the required key');
                    index = [index;-1;-1];
                end
            end
            % create and node and insert to andLayer;
            if( sum(abs(index))==0)
		continue;
	    end
	    node= index';
            %{ comment because same node won't appear in practice
            loc = findMember(andLayer{iLevel},node);
            if isempty(loc) % add node
                andLayer{iLevel}= [andLayer{iLevel};node];
                loc = size(andLayer{iLevel},1);
            elseif size(loc)>1 % report error
                disp('error, more than one and node has the required key');
                loc = -1;
            end
            %}
            and_layer = [and_layer;node];
            loc = size(and_layer,1);
            children = [children;loc];
        end %jVol
        orNode.children = children;
        orLayer{iLevel}(iVol) = orNode;
    end
    andLayer{iLevel}=and_layer;
end


%% enlarge the planes in plane list, to accomodate deformation and padding
for iPlane = 1:length(planeList);
    plane = planeList(iPlane);
    plane=enLargePlane(plane,1.2);
    planeList(iPlane) = plane;
end

%save('text.mat','termLayer')

disp('export to graphviz...')
%expGraph(andLayer,orLayer,termLayer)



numAnd = sum(cell2mat(cellfun(@(x){size(x,1)},andLayer)));
numOr = sum(cell2mat(cellfun(@(x){size(x,2)},orLayer)));
nPlane = size(ntList,1);
fprintf(' %d AND nodes, %d OR nodes, %d planes\n',numAnd,numOr,nPlane);

AoT.andLayer = andLayer;
AoT.orLayer = orLayer;
AoT.termLayer = termLayer;
AoT.ntList = ntList;
AoT.planeList = planeList;
AoT.rotationMat = rotationMat;
AoT.origin = origin;
AoT.dim = [W;H;L];
AoT.swt = swt;
end % end of fucntion createTree;



%% functions for or layer
function keyArray=createKey(orArray)
keyArray = arrayfun(@(x){[x.corner;x.dim]'},orArray);
keyArray = cell2mat( keyArray');
keyArray = int32(keyArray);
end

%% function for and layer


%% functions for terminal layer
function termVolArray=changeVolArrayToTerm(volArray)
termVolArray=arrayfun(@changeToTerm, volArray);
end

function node = changeToTerm(node)
node.type = 'Term';
end % end chnageToTerm


function [node, planeList, ntList]= addPlane(node,planeList,ntList,stp)
% check for vertical plane
if prod(node.dim)==0
    nt = [0;0;1;-node.corner(3)];
    % created to compute palne.v
    pt4 = [0;0;0];
    pt1 = [0;1;0];
else
    % create 4 points, and solve surface normal
    pt1 = node.corner + [0;node.dim(2);0];
    pt2 = node.corner + [node.dim(1:2);0];
    pt3 = node.corner + [node.dim(1);0;node.dim(3)];
    pt4 = node.corner + [0;0;node.dim(3)];
    A= [pt1';pt2';pt3';pt4'];
    B = ones(4,1);
    n = A\B;
    nt = [n;-1];
    scale= norm(nt(1:3));
    nt = nt/scale;
    %% adjust sign of nt
    pt5 = node.corner + [0; node.dim(2:3)];
    if (nt'*[pt5;1])<0
        nt = -nt;
    end
    
end
%
plane = createPlane(nt,node.corner+[0;node.dim(2);0]);
plane.w = node.dim(1); % node.w
plane.h = norm(node.dim(2:3)); % node.h^2+node.l^2
plane.v = eye(3,1);%% update % previousely it is [1;0;0]
plane.u = pt4-pt1; %% update
plane.u = plane.u/norm(plane.u); %% update
%[index planeList ntList] =insertPlane(plane,planeList,ntList);
plane.ntIndex = 1;
plane.stp = stp;
node.plane = plane;
end



%% functions for generic volume
function node = createVol(corner, dim,type)
if ~strcmp(type,'or') &&~strcmp(type,'and') && ~strcmp(type,'term');
    disp('waring, node type other than defined created');
    disp('return nil vol')
    node = [];
    return;
end
%{
node.type = type; % and, or, terminal, root
node.corner = corner; % coordinate along X,Y,Z axis
node.dim = dim; % dimension along X,Y,Z
node.children = [];
node.parent =[];
node.R = [];
%}
node = struct('type',type,'corner',corner,'dim',dim,'children',[],'parent',[],'R',[],'plane',[]);


% extra attributes for 'or' node

% extra attributes for 'and' node

% extra attributes for 'term' node
%node.plane=[];% param for the plane associated with the node
end


%% functions for generic plane;
function plane = createPlane(nt,origin)
plane.nt=nt;% 4 by 1 surface normal  (nt)^t x = 0
plane.origin = origin; % planes origin in root vol coordinate system
plane.u = []; % first axis in root coordinate system, it's a constant
plane.v = [];
plane.w = [];
plane.h = [];
plane.ntIndex =[];
plane.stp = 1;
end

function [index planeList ntList]=insertPlane(newPlane,planeList,ntList)
% test if exist same surface normal in planeList
nt = newPlane.nt;
loc = findMember(ntList,nt');

%res =cell2mat(cellfun(@(x){sum(abs(x-nt))},ntList));
%index = find(res==0);
% if not exist, insert;
% if exist, enlarge the plane to contain current plane.
if isempty(index);
    planeList=[ planeList;newPlane];
    %ntList{end+1}=nt;
    ntList(end+1,:) = nt';
    index = length(planeList);
elseif length(index)==1;
    currPlane = planeList(index); % existing plane
    tL1 = currPlane.origin; % top left point of existing plane
    tL2 = newPlane.origin;
    newOrigin = [min(tL1(1),tL2(1)); max(tL1(2),tL2(2)); min(tL1(3),tL2(3))];
    
    bR1 = currPlane.origin + [currPlane.w;0;0] + currPlane.h*currPlane.u;%% upate
    bR2 = newPlane.origin + [newPlane.w;0;0;]+ newPlane.h*newPlane.u; %%upate
    newEnd = [max(bR1(1),bR2(1)); min(bR1(2),bR2(2)); max(bR1(3),bR2(3))];
    
    newW = newEnd(1)-newOrigin(1);
    newH =(newEnd-newOrigin)'*currPlane.u; %% upate
    
    
    currPlane.origin = newOrigin;
    currPlane.w = newW;
    currPlane.h = newH;
    planeList(index)=currPlane;
else
    disp('warning, more than one plane is simiar to current plane');
    index=[];
    return;
end

end

function plane =  enLargePlane(plane,ratio)
origin = plane.origin;
origin = origin - (ratio-1)*0.5*plane.w*plane.v;%% update
origin = origin - (ratio-1)*0.5*plane.h*plane.u;%% update
plane.origin = origin;
plane.w = ratio*plane.w;
plane.h = ratio*plane.h;
end




% function to export graph to graph viz for testing
function expGraph(andLayer,orLayer,termLayer)
pFile = fopen('showAoT.dot','w+');
fprintf(pFile,'digraph AoT{\n');
%fprintf(pFile,'O_0_0_0_0_0_0 [label = "nil"]\n');
%% or layer
for iLevel = 1:size(orLayer,1)
    for iNode = 1:length(orLayer{iLevel});
        node = orLayer{iLevel}(iNode);
        fprintf(pFile,'O_%d_%d [label = "%d_%d_%d_%d_%d_%d" shape = "ellipse" style = "filled" ]\n',...
            iLevel,iNode,...
            node.corner(1),node.corner(2),node.corner(3),...
            node.dim(1),node.dim(2),node.dim(3));
        nodeName = ['O_' num2str(iLevel) '_' num2str(iNode)];
        fprintf(pFile, '%s -> T_%d_%d\n',nodeName,iLevel,iNode);
        for iChd = 1:length(node.children)
            fprintf(pFile, '%s -> A_%d_%d\n',nodeName,iLevel,node.children(iChd));
        end
    end
end
%% and layer
for iLevel = 1:size(orLayer,1)
    for iNode = 1:size(andLayer{iLevel},1);
        node = andLayer{iLevel}(iNode,:);
        fprintf(pFile,'A_%d_%d [ label = "%d_%d_%d_%d" shape = "ellipse"]\n',...
            iLevel,iNode,...
            node(1),node(2),node(3),node(4));
        nodeName = ['A_' num2str(iLevel) '_' num2str(iNode)];
        if node(1)~=0 && node(2) ~=0
            fprintf(pFile,'%s -> O_%d_%d\n',nodeName,node(1),node(2));
        end
        fprintf(pFile,'%s -> O_%d_%d\n',nodeName,node(3),node(4));
    end
end

%% terminal layer
for iLevel = 1:size(orLayer,1)
    for iNode = 1:length(orLayer{iLevel});
        node = termLayer{iLevel}(iNode);
        fprintf(pFile,'T_%d_%d [label = "%d_%d_%d_%d_%d_%d" shape = "boxed"]\n',...
            iLevel,iNode,...
            node.corner(1),node.corner(2),node.corner(3),...
            node.dim(1),node.dim(2),node.dim(3));
    end
end


fprintf(pFile,'}\n');
fclose(pFile);
end


%% utility function:
function ind = findMember_deprecated(list,pattern)
% create hashing
hash = sum(list,2);
ind = find(hash==sum(pattern));
list2 = list(ind,:);

% check very simple cases
if size(list2,1) ==0
    ind =[];
    return
elseif size(list2,1)==1
    for i = 1:size(list2,2)
        if list2(i)~=pattern(i)
            ind=[];
            return;
        end
    end
    return;
end
% throw complicated cases to ismember
res = ismember(list2,pattern,'rows');
ind = ind(res);
end


function ind = findMember(list,pattern)
pattern = (pattern);
if isempty(list)
    ind = [];
    return;
end

ind = 1:size(list,1);

%for i = length(pattern):-1:1
for ip = 1:length(pattern)
    column = list(:,ip);
    ind1 =( column(ind)==pattern(ip) );
    ind = ind(ind1);
    if isempty(ind)
        return;
    end
end
end



